#include "stm32_mcu.h"
#include "hardware_api.h"
#ifdef STM32
#include "main.h"

//data limit [0,1]
float constraint(float val){
  if (val>1)val=1;
  else if (val<0)val=0;

  return val;
}


void _init3PWM(){
  HAL_GPIO_WritePin(drv8313_en_GPIO_Port, drv8313_en_Pin, GPIO_PIN_SET);

  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  _writeDutyCycle3PWM(0,0,0);
}

/**
 * @brief write duty cycle for 3 phase motor
 * @param dc_a [0,1]
 * @param dc_b [0,1]
 * @param dc_c [0,1]
 * @param params
 */
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c) {
    // transform duty cycle from [0,1] to [0,_PWM_RANGE]
    dc_a= constraint(dc_a);
    dc_b= constraint(dc_b);
    dc_c= constraint(dc_c);

    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, _PWM_RANGE*dc_a);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, _PWM_RANGE*dc_b);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, _PWM_RANGE*dc_c);
}

#endif